﻿// <copyright file="Matrix.cs" company="Community Combat Simulator">
//    Community Combat Simulator: a free combat flight simulator.
//    Copyright (C) 2008 Aaron Schinkowitch
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see http://www.gnu.org/licenses/.
// </copyright>
namespace CCSim.Vecmath
{
    using System;
    using System.Globalization;
    using CCSim.Util;

    /// <summary>
    /// A 3x3 matrix with double precision.
    /// </summary>
    public class Matrix
    {
        /// <summary>
        /// Equality factor to use when comparing matrix elements for equality.
        /// </summary>
        private const double EqualityFactor = 1E-5;
        
        /// <summary>
        /// Element at row 0, column 0.
        /// </summary>
        private double m00;

        /// <summary>
        /// Element at row 0, column 1.
        /// </summary>
        private double m01;
        
        /// <summary>
        /// Element at row 0, column 2.
        /// </summary>
        private double m02;
        
        /// <summary>
        /// Element at row 1, column 0.
        /// </summary>
        private double m10;
        
        /// <summary>
        /// Element at row 1, column 1.
        /// </summary>
        private double m11;
        
        /// <summary>
        /// Element at row 1, column 2.
        /// </summary>
        private double m12;
        
        /// <summary>
        /// Element at row 2, column 0.
        /// </summary>
        private double m20;
        
        /// <summary>
        /// Element at row 2, column 1.
        /// </summary>
        private double m21;
        
        /// <summary>
        /// Element at row 2, column 2.
        /// </summary>
        private double m22;
        
        /// <summary>
        /// Initializes a new instance of the Matrix class as an identity matrix.
        /// </summary>
        public Matrix()
        {
            this.M00 = 1.0;
            this.M11 = 1.0;
            this.M22 = 1.0;
        }
        
        /// <summary>
        /// Initializes a new instance of the Matrix class copying the element values of
        /// the specified matrix.
        /// </summary>
        /// <param name="other">The matrix whose element values will be copied.</param>
        public Matrix(Matrix other)
        {
            this.SetElements(other);
        }

        /// <summary>
        /// Gets or sets the element at row 0, column 0.
        /// </summary>
        internal double M00 
        {
            get { return this.m00; }
            set { this.m00 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 0, column 1.
        /// </summary>
        internal double M01 
        {
            get { return this.m01; }
            set { this.m01 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 0, column 2.
        /// </summary>
        internal double M02 
        {
            get { return this.m02; }
            set { this.m02 = value; }
        }

        /// <summary>
        /// Gets or sets the element at row 1, column 0.
        /// </summary>
        internal double M10 
        {
            get { return this.m10; }
            set { this.m10 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 1, column 1.
        /// </summary>
        internal double M11 
        {
            get { return this.m11; }
            set { this.m11 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 1, column 2.
        /// </summary>
        internal double M12
        {
            get { return this.m12; }
            set { this.m12 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 2, column 0.
        /// </summary>
        internal double M20 
        {
            get { return this.m20; }
            set { this.m20 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 2, column 1.
        /// </summary>
        internal double M21 
        {
            get { return this.m21; }
            set { this.m21 = value; }
        }
        
        /// <summary>
        /// Gets or sets the element at row 2, column 2.
        /// </summary>
        internal double M22 
        {
            get { return this.m22; }
            set { this.m22 = value; }
        }
        
        /// <summary>
        /// Gets the determinant of the matrix.
        /// </summary>
        internal double Determinant 
        {
            get 
            {
                // m00(m11m22-m21m12) - m10(m01m22-m21m02) + m20(m01m12-m11m02)
                return (this.M00 * ((this.M11 * this.M22) - (this.M21 * this.M12)))
                    - (this.M10 * ((this.M01 * this.M22) - (this.M21 * this.M02)))
                    + (this.M20 * ((this.M01 * this.M12) - (this.M11 * this.M02)));
            }
        }
        
        /// <summary>
        /// Sets the elements of the matrix to equal those of the specified matrix.
        /// </summary>
        /// <param name="other">The matrix to copy.</param>
        /// <returns>The matrix itself.</returns>
        public Matrix SetElements(Matrix other)
        {
            if (other == null) 
            {
                throw new ArgumentNullException("other");
            }
            
            this.M00 = other.M00;
            this.M01 = other.M01;
            this.M02 = other.M02;
            this.M10 = other.M10;
            this.M11 = other.M11;
            this.M12 = other.M12;
            this.M20 = other.M20;
            this.M21 = other.M21;
            this.M22 = other.M22;
            
            return this;
        }
        
        /// <summary>
        /// Checks for equality with another object.
        /// </summary>
        /// <param name="obj">The object to check.</param>
        /// <returns>true if the object is equal to the matrix, false otherwise.</returns>
        public override bool Equals(object obj)
        {
            Matrix other = obj as Matrix;
            
            Equality tester = new Equality();
            
            return other != null 
                && tester.RoughlyEqual(this.M00, other.M00, EqualityFactor)
                && tester.RoughlyEqual(this.M01, other.M01, EqualityFactor)
                && tester.RoughlyEqual(this.M02, other.M02, EqualityFactor)
                && tester.RoughlyEqual(this.M10, other.M10, EqualityFactor)
                && tester.RoughlyEqual(this.M11, other.M11, EqualityFactor)
                && tester.RoughlyEqual(this.M12, other.M12, EqualityFactor)
                && tester.RoughlyEqual(this.M20, other.M20, EqualityFactor)
                && tester.RoughlyEqual(this.M21, other.M21, EqualityFactor)
                && tester.RoughlyEqual(this.M22, other.M22, EqualityFactor);
        }
        
        /// <summary>
        /// Gets a hash code for the matrix.
        /// </summary>
        /// <returns>A hash code.</returns>
        public override int GetHashCode()
        {
            return this.M00.GetHashCode() ^ this.M11.GetHashCode() ^ this.M22.GetHashCode();
        }
        
        /// <summary>
        /// Gets a string representation of the matrix.
        /// </summary>
        /// <returns>A string representation of the matrix.</returns>
        public override string ToString()
        {
            return String.Format(
                CultureInfo.InvariantCulture,
                "{0} (0,0: {1} 0,1: {2} 0,2: {3} 1,0: {4} 1,1: {5} 1,2: {6} 2,0: {7} 2,1: {8} 2,2: {9})",
                GetType().Name, 
                this.M00, 
                this.M01, 
                this.M02, 
                this.M10, 
                this.M11, 
                this.M12, 
                this.M20, 
                this.M21, 
                this.M22);
        }
    
        /// <summary>
        /// Scales the matrix in place.
        /// </summary>
        /// <param name="scalar">The scalar by which to scales the elements of the matrix.</param>
        /// <returns>The matrix itself.</returns>
        public Matrix Scale(double scalar) 
        {
            if (Double.IsNaN(scalar) || Double.IsInfinity(scalar)) 
            {
                throw new ArgumentOutOfRangeException("scalar is " + scalar);
            }
            
            this.M00 *= scalar;
            this.M01 *= scalar;
            this.M02 *= scalar;
            this.M10 *= scalar;
            this.M11 *= scalar;
            this.M12 *= scalar;
            this.M20 *= scalar;
            this.M21 *= scalar;
            this.M22 *= scalar;
            
            return this;
        }
                
        /// <summary>
        /// Sets the elements of the matrix to be the rotation matrix defined
        /// by the supplied rotation quaternion.
        /// </summary>
        /// <param name="rotation">The rotation quaternion. Must be a unit quaternion.</param>
        /// <returns>The matrix itself.</returns>
        public Matrix SetRotation(Quaternion rotation)
        {
            if (rotation == null)
            {
                throw new ArgumentNullException("rotation");
            }

            if (!rotation.IsNormalized())
            {
                throw new ArgumentOutOfRangeException(
                    "rotation", "Rotation quaterion was not normalized " + rotation);
            }
            
            /*
                Given a quaternion q [w; x, y, z], quaternion_to_matrix returns the matrix
                
                1 −2y² −2z², 2xy − 2wz, 2xz + 2wy
                2xy + 2wz, 1 −2x² −2z², 2yz − 2wx
                2xz −2wy, 2yz + 2wx, 1 − 2x² − 2y²
            */
           double twoX = 2 * rotation.X;
           double twoY = 2 * rotation.Y;
           double twoZ = 2 * rotation.Z;
           
           double twoWX = twoX * rotation.W;
           double twoWY = twoY * rotation.W;
           double twoWZ = twoZ * rotation.W;
           double twoXX = twoX * rotation.X;
           double twoXY = twoX * rotation.Y;
           double twoXZ = twoX * rotation.Z;
           double twoYY = twoY * rotation.Y;
           double twoYZ = twoY * rotation.Z;
           double twoZZ = twoZ * rotation.Z;
           
           this.M00 = 1 - twoYY - twoZZ;   // 1 −2y² −2z²
           this.M01 = twoXY - twoWZ;       // 2xy − 2wz
           this.M02 = twoXZ + twoWY;       // 2xz + 2wy
           this.M10 = twoXY + twoWZ;       // 2xy + 2wz
           this.M11 = 1 - twoXX - twoZZ;   // 1 −2x² −2z²
           this.M12 = twoYZ - twoWX;       // 2yz − 2wx
           this.M20 = twoXZ - twoWY;       // 2xz −2wy
           this.M21 = twoYZ + twoWX;       // 2yz + 2wx
           this.M22 = 1 - twoXX - twoYY;   // 1 − 2x² − 2y²
           
           return this;
        }
        
        /// <summary>
        /// Transposes the matrix inline. After calling this method the rows of the matrix hold 
        /// what was previously the values of the columns of the matrix.
        /// </summary>
        /// <returns>The matrix itself.</returns>
        public Matrix Transpose()
        {
            double temp = this.M10;
            this.M10 = this.M01;
            this.M01 = temp;
            
            temp = this.M20;
            this.M20 = this.M02;
            this.M02 = temp;
            
            temp = this.M21;
            this.M21 = this.M12;
            this.M12 = temp;
            
            return this;
        }
        
        /// <summary>
        /// Performs inline matrix multiplication. The matrix is set to the result of multiplying
        /// itself with the specified matrix.
        /// </summary>
        /// <param name="right">The right hand matrix with which to multiply this matrix by.</param>
        /// <returns>The matrix itself.</returns>
        public Matrix MultiplyBy(Matrix right)
        {
            if (right == null)
            {
                throw new ArgumentNullException("right");
            }
            
            double m0 = this.M00;
            double m1 = this.M01;
            
            this.M00 = (m0 * right.M00) + (m1 * right.M10) + (this.M02 * right.M20);
            this.M01 = (m0 * right.M01) + (m1 * right.M11) + (this.M02 * right.M21);
            this.M02 = (m0 * right.M02) + (m1 * right.M12) + (this.M02 * right.M22);
            
            m0 = this.M10;
            m1 = this.M11;
            
            this.M10 = (m0 * right.M00) + (m1 * right.M10) + (this.M12 * right.M20);
            this.M11 = (m0 * right.M01) + (m1 * right.M11) + (this.M12 * right.M21);
            this.M12 = (m0 * right.M02) + (m1 * right.M12) + (this.M12 * right.M22);
            
            m0 = this.M20;
            m1 = this.M21;
            
            this.M20 = (m0 * right.M00) + (m1 * right.M10) + (this.M22 * right.M20);
            this.M21 = (m0 * right.M01) + (m1 * right.M11) + (this.M22 * right.M21);
            this.M22 = (m0 * right.M02) + (m1 * right.M12) + (this.M22 * right.M22);
            
            return this;
        }
        
        /// <summary>
        /// Checks if the matrix can be inverted.
        /// </summary>
        /// <returns>true if the matrix can be inverted, false otherwise.</returns>
        public bool IsInvertible() 
        {
            return this.Determinant != 0.0;
        }
        
        /// <summary>
        /// Inverts the matrix inline. The matrix must be invertible. The inverse of a matrix
        /// is defined such that multiplying a matrix by its inverse.
        /// </summary>
        /// <returns>The matrix itself.</returns>
        public Matrix Invert() 
        {
            double determinant = this.Determinant;
            
            if (determinant == 0.0)
            {
                throw new InvalidOperationException("Not invertible: determinant is 0");
            }
            
            this.Adjoint().Scale(1.0 / determinant);
            
            return this;
        }
        
        /// <summary>
        /// Sets the elements of the matrix to the adjoint of the matrix inline.
        /// </summary>
        /// <returns>The matrix itself.</returns>
        internal Matrix Adjoint() 
        {
            /*
                |   m11*m22 - m12*m21  m02*m21 - m01*m22  m01*m12 - m02*m11 |
                |   m12*m20 - m10*m22  m00*m22 - m02*m20  m02*m10 - m00*m12 |
                |   m10*m21 - m11*m20  m01*m20 - m00*m21  m00*m11 - m01*m10 |            
            */
           
            double m00 = (this.M11 * this.M22) - (this.M12 * this.M21);
            double m01 = (this.M02 * this.M21) - (this.M01 * this.M22);
            double m02 = (this.M01 * this.M12) - (this.M02 * this.M11);
            
            double m10 = (this.M12 * this.M20) - (this.M10 * this.M22);
            double m11 = (this.M00 * this.M22) - (this.M02 * this.M20);
            double m12 = (this.M02 * this.M10) - (this.M00 * this.M12);
            
            double m20 = (this.M10 * this.M21) - (this.M11 * this.M20);
            double m21 = (this.M01 * this.M20) - (this.M00 * this.M21);
            double m22 = (this.M00 * this.M11) - (this.M01 * this.M10);
            
            this.M00 = m00;
            this.M01 = m01;
            this.M02 = m02;
            this.M10 = m10;
            this.M11 = m11;
            this.M12 = m12;
            this.M20 = m20;
            this.M21 = m21;
            this.M22 = m22;
            
            return this;
        }
    }
}
